#!/usr/bin/env python

import sys, rospy, cv2, time
import numpy as np
from lsr import listener

from std_msgs.msg import Float64
from geometry_msgs.msg import Twist


class Manipulation:
	def __init__(self):
		rospy.on_shutdown(self.clean_up)

		
		
		self.joint_tilt = rospy.Publisher('/tilt_controller/command', Float64, queue_size=10)
		self.joint_shoulder = rospy.Publisher('/shoulder_controller/command', Float64, queue_size=10)
		self.joint_elbow = rospy.Publisher('/elbow_controller/command', Float64, queue_size=10)
		self.joint_wrist = rospy.Publisher('/wrist_controller/command', Float64, queue_size=10)
		#self.joint_wrist_turn = rospy.Publisher('/wrist_turn_controller/command', Float64, queue_size=10)
		self.joint_hand = rospy.Publisher('/hand_controller/command', Float64, queue_size=10)
		rospy.sleep(2)
		self.pose = Float64()


	def move_tilt(self, pose):
		self.pose = pose
		self.joint_tilt.publish(self.pose)
		#rospy.sleep(3)


	def move_shoulder(self, pose):
		self.pose = pose
		self.joint_shoulder.publish(self.pose)
		#rospy.sleep(3)


	def move_elbow(self, pose):
		self.pose = pose
		self.joint_elbow.publish(self.pose)
		#rospy.sleep(3)


	def move_wrist(self, pose):
		self.pose = pose
		self.joint_wrist.publish(self.pose)
		#rospy.sleep(3)


	def move_hand(self, pose):
		self.pose = pose
		self.joint_hand.publish(self.pose)
		#rospy.sleep(3)
		

	def clean_up(self):
		rospy.loginfo('Shutting down robot arm.')


	def init_arm(self):
		self.move_tilt(1)
		self.move_shoulder(0.5)
		self.move_elbow(5.1)
		self.move_wrist(2.3)
		self.move_hand(2.0)
		rospy.sleep(5)



	def prepare(self):
		self.move_tilt(1)
		self.move_shoulder(2.6)
		self.move_elbow(2.6)
		self.move_wrist(4.2)
		self.move_hand(3.1)
		rospy.sleep(5)

	def front(self):
		self.move_wrist(4)
		rospy.sleep(1)

	def back(self):
		self.move_wrist(3)
		rospy.sleep(1)

	def open(self):
		self.move_hand(3.8)
		rospy.sleep(1)

	def grasp(self):
		self.move_hand(2.2)
		rospy.sleep(1)

	
	def lift(self):
		self.move_tilt(1)
		self.move_shoulder(2.6)
		self.move_elbow(2.6)
		self.move_wrist(4.2)
		self.move_hand(3.1)
		rospy.sleep(5)


	def put_down(self):
		self.move_tilt(2.6)
		self.move_shoulder(0.8)
		self.move_elbow(1.2)
		self.move_wrist(0.8)
		self.move_hand(2.2)
		rospy.sleep(5)


	def put_down2(self):
		self.move_hand(2.2)
		rospy.sleep(5)


	def dump_prepare(self):
		self.move_shoulder(2.8)
		self.move_elbow(0)
		self.move_wrist(1.6)
		self.move_hand(3.2)
		rospy.sleep(5)


	def dump(self):
		self.move_wrist_turn(2.4)
		rospy.sleep(25)

	def give(self):
		# self.move_tilt(1)
		self.move_shoulder(1)
		self.move_elbow(3.6)
		self.move_wrist(3.6)
		self.move_hand(2.0)
		rospy.sleep(5)

	def move1(self):
		self.prepare()
		self.front()
		self.back()
		self.front()
		self.back()
		self.give()
		self.open()
		self.grasp()
		self.open()
		self.grasp()
		rospy.sleep(5)


			

if __name__ == '__main__':
	rospy.init_node('photo', anonymous=False)
	cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
	move_cmd = Twist()
	manipulation = Manipulation()
	listen = listener()
	order = 5
	while True:
		order = listen.meet()
		rospy.sleep(1)
		if order == 1:
			manipulation.init_arm()
			manipulation.move1()
			manipulation.init_arm()
			break
		if order == 6:
			manipulation.give()
			manipulation.open()
			break
			 		
		else:
			continue
		

   		
	
	

	# manipulation.lift()
    
	# go back after get the coke (for slam)
	# move_cmd.linear.x = - 0.05
	move_cmd.angular.z = 0.0

	for i in range(30):
		cmd_vel.publish(move_cmd)
		rospy.sleep(0.1)

	move_cmd.linear.x = 0.0
	move_cmd.angular.z = 0.0
	cmd_vel.publish(move_cmd)
	rospy.sleep(1)
	listen.help()
	while True:
		order = listen.meet()
		rospy.sleep(1)
		if order == 7:
			manipulation.give()
			manipulation.open()
			rospy.sleep(3)
			manipulation.init_arm()
			break
			 		
		else:
			continue
	listen.help()
	

